#!/usr/bin/env python
#-*- coding=UTF-8 -*-
import os
import sys
import pdb

def format_pose(path, file_name_for_read, file_name_for_write):
    w_path = os.environ['HOME'] + '/' + path + '/' + file_name_for_write
    r_path = os.environ['HOME'] + '/' + path + '/' + file_name_for_read
    flag = False
    try:
        f_w = open(w_path, 'a+')
        f_r = open(r_path, 'r+')
        flag = True
        extracts = f_r.readlines()
        x = 0
        remain_rows = []
        while x < len(extracts):
            remain_rows.append(x+3)
            remain_rows.append(x+9)
            remain_rows.append(x+10)
            remain_rows.append(x+11)
            remain_rows.append(x+13)
            remain_rows.append(x+14)
            remain_rows.append(x+15)
            remain_rows.append(x+16)
            x += 17
        i = 0
        while i < len(remain_rows):
            extracts[remain_rows[i]] = extracts[remain_rows[i]].replace('\n','')
            f_w.write(extracts[remain_rows[i]])
            f_w.write(' ')
            if (i+1)%8 == 0:
                f_w.write('\n')
            i += 1
       # for i in range(len(remain_rows)):
            #f_w.write(extracts[remain_rows[i]])

    except:
        print('文件出错')
    finally:
        if flag:
            f_w.close()
            f_r.close()
        else:
            print('退出')
            sys.exit(1)

f_r = raw_input('需要格式化的文件:')
f_w = raw_input('目标文件:')
path = 'sensor_msgs_of_carto'
format_pose(path, f_r, f_w)
